//
// Created by Aron on 2024/10/31.
//

#include "Elevator.h"
#include "Speed_control.h"
#include "CAN_receive.h"
#include "remote_control.h"
pid_type_def pid_speed_Elevator;
pid_type_def pid_angle_Elevator;
static float n_E=4.3945f;
static int16_t max_speed_change_E=2000;
int ElevatorFlag=1;
static float hlimit_E;
static float llimit_E;
static float set_angle;
static float angle_E;
static float ref_angle;
void Elevator_control(int16_t angle) {
    if (rc_ctrl.rc.s[0]==1) {
        if (ElevatorFlag) {

            set_angle = motor_measure[3].total_ecd * n_E;
            ElevatorFlag = 0;
        } else set_angle += angle;
        ref_angle = motor_measure[3].total_ecd * n_E;
//    if (set_angle>hlimit_E) {
//        set_angle=hlimit_E;
//    }else if(set_angle<llimit_E) {
//        set_angle=llimit_E;
//    }
        PID_calc(&pid_angle_Elevator, ref_angle, set_angle);
        Speed_out(pid_angle_Elevator.out, motor_measure[3].speed_rpm, max_speed_change_E,
                  &pid_speed_Elevator);
    }else{
        ref_angle = motor_measure[3].total_ecd * n_E;
        PID_calc(&pid_angle_Elevator, ref_angle, set_angle);
        Speed_out(pid_angle_Elevator.out, motor_measure[3].speed_rpm, max_speed_change_E,
                  &pid_speed_Elevator);
    }
}